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动态 环境 下 基于 人 工 势 场 引 导 的 RRT 路 径 规 划算 法 
司徒 华 杰 ， 雷 海 波 ， 庄 春 刚 


(上 海 交通 大 学 机 械 与 动力 工程 学 院 ， 上 海 200240) 


摘 要 : 现 有 的 大 多 数 动态 RRT 路 径 规划 算法 不 能 使 规划 的 路 径 远离 障碍 物 ， 这 有 可 能 导致 机 器 人 没有 足够 的 避 障 
时 间 。 针 对 这 个 问题 ,提出 了 一 种 利用 人 工 势 场 引导 快速 扩展 随机 树 向 目标 区 域 生 长 并 远离 障碍 物 的 改进 RRT | 
APFG-RRT(artificial potential field guided RRT)。 然 后 , 为 了 进一步 加 快 算法 的 收敛 速度 ,加 速算 法 跳出 局 部 极 小 

引入 了 一 种 按 自 适 应 概率 选择 目标 点 作为 采样 点 的 策略 。 最 后 ， 针 对 动态 环境 采用 全 局 规划 结合 局 部 重新 规划 I 
法 ， 以 提高 算法 的 实时 性 。 念 真实 验 表 明 ， 相 比 于 初始 RRT 和 Goal-bias RRT，APFG-RRT 的 计算 效率 更 高 ， 内 存 
需求 更 小 ， 并 且 搜 索 到 的 路 径 能 够 有 效 地 远离 障碍 物 ， 提 高 了 动态 路 径 规划 的 成 功率 。 
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Artificial potential field based RRT algorithm for path planning in dynamic environment 


Situ Huajie, Lei Haibo, Zhuang Chungang 
(School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China) 


Abstract: Most of the existing dynamic RRT path planning algorithms cannot keep the planned path away from obstacles, 
which may cause the robot to have insufficient time to avoid obstacles. To solve this problem, this paper proposed an improved 
RRT, denoted as APFG-RRT, which utilized Artificial Potential Fields to guide the RRT grow to goal and away from obstacles. 
Then, in order to further increase the convergence rate and speed up the jump out of local minima, it introduced a strategy of 
selecting the goal as the random sample at an adaptive probability. Finally, it adopted global planning combined with local 
replanning to improve its real-time performance in dynamic environment. Simulation experiments show that APFG-RRT has 
higher computational efficiency and lower memory requirements compared with the initial RRT and Goal-bias RRT, and the 
given path can be effectively away from obstacles, which improves the success rate of dynamic path planning. 
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0 引言 长 的 Goal-piasRRT05; 周秀 等 人 引入 贪 禁 生 长 的 策略 559, 每 
次 生长 采用 尽 可 能 大 的 步 长 ， 从 而 快速 覆盖 更 多 区 域 。 但 上 
路 径 规划 是 在 给 定 的 环境 下 寻找 一 条 从 初始 位 置 到 目标 看 提 到 的 算法 都 没有 利用 环境 信息 ， 为 了 引进 环境 信息 对 


位 置 的 无 碰撞 路 径 的 过 程 , 其 在 机 器 人 系统 趾 、 医 学 争 、 计算。 RRT 的 引导 ， 一 些 研究 人 员 探 索 了 将 RRT 与 人 工 势 场 结 合 
机 动画 中 、 现 代 工 业 等 众多 领域 应 用 广泛 。 根 据 对 环境 信息 ”的 方法 。 文 献 [15,16] 通 过 切换 两 种 算法 将 两 者 结合 起 来 : 当 
的 掌握 程度 不 同 ， 路 径 规划 可 以 分 为 全 局 路 径 规划 和 局 部 路 。 人 工 势 场 法 规划 路 径 陷 入 局 部 极 小 值 时 切换 至 RRT 规划 , T 
径 规 划 。 局 部 路 径 规划 算法 有 神经 网 络 外 、 人 工 势 场 法 四 、 遗 。 当 跳 出 局 部 极 小 值 时 再 切换 回 人 工 势 场 法 ， 文 献 [17,18] 提 出 
传 算法 [9 等 , 其 中 人 工 势 场 法 因 其 算法 简单 、 实 时 性 好 、 规划 ”将 随机 采样 点 向 目标 点 方向 偏 移 的 改进 RRT* 算 法 ， 偏 移 量 
的 路 径 平 滑 等 优点 而 受到 广泛 关注 。 但 人 工 势 场 法 在 规划 路 。 与 采样 点 和 障碍 物 之 间 的 最 小 距离 相关 ， 隐 性 地 引入 了 斥 力 
径 时 可 能 会 陷入 局 部 极 小 值 ， 并 且 容 易 在 目标 点 附近 发 生 的 大 小 ; 文献 [19] 将 文献 [18] 的 算法 扩展 到 B-RRT* 中 。 由 于 
荡 ， 这 大 大 限制 了 它 的 实际 应 用 。 全 局 路 径 规划 的 方法 有 ! 这 些 算法 是 基于 RRT* 进 行 改进 ,它们 致力 于 搜索 最 优 路 径 ， 
群 算法 中 、 RRT、 可 视图 法 9 等 ， nnn mi RRT 通 实时 性 较 差 不 适用 于 实时 性 要 求 较 高 的 动态 路 径 规划 。 并 
过 随机 采样 快速 探索 构 型 空间 ， 有 很 高 的 计算 效率 ， 且 具有 且 这 些 算法 也 不 适合 直接 移植 到 只 需 规划 出 一 条 路 径 的 动态 
概率 完备 性 ， 即 当 规 划 的 时 间 趋 向 于 无 穷 时 ， RRT 算法 中 ， 因 为 这 些 算法 利用 引力 场 和 斥 力 的 大 小 对 采样 
可 行路 径 的 概率 趋向 于 1。 点 进行 偏 移 ， 当 随机 树 上 节点 较 少 时 ， 采 样 点 一 般 与 待 生长 
为 了 进一步 提高 RRT 的 收敛 速度 ,许多 研究 人 员 在 RRT ”节点 距离 较 远 ， 采样 点 周围 的 势 场 不 能 表示 待 生 长 节点 周 
的 基础 上 进行 改进 。Kalisiak 等 人 提出 RRT-blossom[11, 用 下 的 环境 信息 ， 斥 力 对 算法 的 引导 作用 较 小 ， 只 有 当 随机 树 节 
归 约 束 函 数 生成 新 节点 ， 降 低 随 机 树 前 期 重复 探索 同一 点 在 搜索 空间 密集 分 布 时 ， 采 样 点 周围 的 势 场 才 近似 等 于 待 
的 概率 ; Shkolnik 等 人 用 超 球体 代替 原来 无 体积 的 节点 发 长 节点 周围 的 势 场 。 而 且 这 些 算法 没有 引入 斥 力 的 方向 ， 
了 Ball Tree00， 通 过 拒绝 节点 内 的 采样 点 使 随机 树 专 注 于 这 只 能 减缓 随机 树 向 障碍 物 的 生长 ， 而 不 能 使 随机 树 远 离 障 
索 还 未 探索 的 区 域 ; Lavalle 等 人 提出 了 BidirectionalRRT02， ”得 物 ， 这 有 可 能 使 规划 的 路 径 贴 近 动 态 障碍 物 表 面 而 有 发 生 
通过 在 初始 位 置 和 目标 位 置 同时 扩展 随机 树 来 加 速 探 索 速度 ， ”碰撞 的 危险 。 除 此 之 外 ， 这 些 算法 的 生长 步 长 都 是 固定 的 ， 
随后 Lavalle 还 提出 用 目标 点 信息 引导 随机 树 偏 向 目标 点 生 ”无 法 适应 不 同 的 局 部 环境 
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为 了 克服 上 述 问 题 ， 本 文 提出 了 用 势 场 引导 
生长 的 改进 RRT 动态 路 径 规 划算 法 一 一 APFG-RRT， 利 用 构 


J 十 证 |.4 
待 生 习 节点 


的 搜索 漫 无 目的 , 导致 效率 很 低 。 为 此 , 许多 启发 式 搜索 的 RRT 


造 的 引力 势 场 和 斥 力 势 场 使 随机 树 向 目标 靠近 的 同时 远离 障 
碍 物 ， 并 且 在 不 同 的 局 部 环境 下 拥有 自 适 应 的 生长 步 长 。 同 


时 ， 本 文 提出 了 一 种 自 适 应 概率 采样 的 偏向 生长 策略 ， 大 大 
减少 算法 在 局 部 极 小 值 区 域 的 重复 计算 ， 并 匀 


用 了 局 部 重新 规划 的 策略 。 仿 真实 验 表 明 ， 与 
Goal-bias RRT 相 比 ，APFG-RRT 在 包括 狭 窗 慰 


对 动态 环境 使 
原本 的 RRT 和 
[动态 环境 在 内 


算法 被 提出 ，Goal-bias RRT 是 其 中 一 种 简单 有 效 的 算法 。 
初始 的 RRT 首先 将 机 器 人 初始 位 置 mw 设 为 根 节点 ， 然 
后 在 机 器 人 构 型 空间 随机 采样 一 个 点 x ， 选 取 随 机 树 上 离 
xy 最 近 的 节点 mr 作为 待 生长 的 点 , 由 于 RRT 外 部 的 节点 
对 应 的 泰 森 多 边 形 面 积 较 大 ， 所 以 外 部 节点 得 到 生长 的 概率 
更 大 ， 这 使 得 RRT 拥有 快速 向 外 扩展 的 能 力 。 接 着 由 wwe 


NY 


的 各 种 环境 下 都 有 更 高 的 计算 效率 和 更 少 的 节点 数 。 


1 人工 势 场 法 


人 工 势 场 法 是 由 Khatib 提出 的 一 种 虚拟 力 法 。 它 在 机 器 


人 周围 环境 建立 一 个 目标 


= 


区 域 势能 最 小 的 虚拟 势 场 ， 利 用 梯 


度 下 降 引 导 机 器 人 到 达 目 标 区 域 。 具 体 为 在 目标 区 域 构建 一 
个 引力 势 场 Qw ， 并 在 障碍 物 区 域 构建 一 个 斥 力 势 场 ww ， 使 
得 机 器 人 被 吸引 向 目标 区 域 而 被 障碍 物 区 域 排斥 。 引 力 势 函 
数 和 引力 函数 通常 为 
SK,d’ (wx) dC a ) > 
Ua = 1 (1) 
Ka(dsd(x, Xm) 一 ds ) d(x, Xsom) S ds 
—K,(X— Xsoa) d(xX, Xsoa) > ds 
Fn = 加 MX 一 MXgoal 人 # (2) 
“ds A to) d(x,Xgoa) < ds 


其 中 ， 玉 为 引力 常数 ， 工 和 xu 分 别 为 机 器 人 控制 点 位 置 和 
目标 位 置 ，4Q) 表示 两 个 位 置 之 间 的 欧 氏 距离 , 为 设 定 的 距离 
闷 值 。 当 机 器 人 控制 点 与 目标 点 距离 大 于 4d; 时， 引力 势能 与 


距离 的 二 次 方 成 正比 ， 使 机 器 人 快速 向 目标 点 靠近 ;， 当 控 制 


点 与 目标 点 距离 小 于 4 时 ， 引 力 势 场 以 目标 点 为 中 心 呈 锥 形 ， 


一 定 程度 上 缓解 在 目标 点 附近 震荡 的 问题 。 


斥 力 势 函 数 和 斥 力 函数 为 
LRC 1y ual < 
Uwm=12 dnl ad; 

0 dna)> a 
1 1 QI 
-一 )- oo dlSd 

三 d; din| dn 
0 [dal > ad: 


G3) 


(4) 


其 中 ，K, 为 斥 力 常数 ，d 为 由 机 器 人 控制 点 指向 障碍 物 最 


近 点 的 向 量 ，4 为 斥 力 势 场 的 作用 半径 。 
机 器 人 的 总 势能 和 受到 的 合力 为 
Un = Ui + Us 
Piva = Faa + Fo 


(5) 
(6) 


人 工 势 场 的 小 步 长 梯度 下 降 算法 如 下 所 示 ， 
机 器 人 的 初始 位 置 ，s 为 设 定 的 步 长 ,函数 PotentialGradient 


根据 式 (6) 计 算出 所 受 合力 。 
算法 1 人 工 势 场 梯度 下 降 搜索 


1: HO Xi 


2: while VU,a #0: 


3: Fa <— PotentialGradient(x;) 
Lk 

4: Xi + 
[Fa 

5: i=i+l 


2 Goal-bias RRT 


其 中 x 为 


RRT 是 一 种 基于 增 量 采样 的 路 径 规划 算法 ， 


起 始 位 置 建立 根 节 点 ， 通 过 在 机 器 人 构 型 空间 


其 在 机 器 人 


随机 采样 ， 引 


导 随 机 树 探 索 未 覆盖 的 区 域 ， 最 终 找 到 一 条 通 向 目标 点 的 路 
径 。 但 是 由 于 初始 RRT 采样 的 完全 随机 性 , 它 在 构 型 空间 中 


向 sw 方向 以 步 长 生长 得 到 新 的 节点 gw ， 判 断 mw 是 否 与 
障碍 物 发 生 碰撞 ， 若 碰撞 则 舍弃 ， 若 无 碰撞 则 保留 ， 不 断 重 
复 以 上 步骤 直到 随机 树 生长 到 目标 区 域 。 而 Goal-bias-RRT 与 
初始 RRT 的 采样 点 生成 方式 有 所 不 同 ，Goal-bias RRT 按 一 
定 概 率 P 选取 目标 点 sw 作为 采样 点 ， 概 率 1-P 随机 取 点 作 
为 采样 点 ， 从 而 引导 随机 树 向 目标 点 生长 。Goal-bias RRT 的 
伪 代 码 如 算法 2 所 示 。 
算法 2 Goal-bias RRT 
1:T < InitializeTree(x,,) 


2: for i=0 to K: 


FR if random(0,1)<P: 

4 Xrand = Xgoal 

5 else: 

6 Xand = SampleO 

7 Nearest <— NearestNode(T, x,aa ) 
8 Naw <— Steer (Xoarerr» Xrana ) 

9 if CollisionFree(x,,,): 

10: T.add_node(x,,,) 

ls T.add_edge(Xwaon, Knew ) 
12: if dio, Xeon) <€: 
13: break 


3 ”基于 人 工 势 场 引导 的 RRT 路 径 规 划算 法 


3.1 人 工 势 场 引导 节点 生长 

Goal-bias RRT 加 入 了 目标 点 信息 ， 使 RRT 的 搜索 更 有 
的 性 ， 从 而 提高 搜索 效率 。 但 是 ， 它 没有 考虑 到 障碍 物 信 
， 而 障碍 物 信息 在 大 多 场景 都 能 够 获取 。 为 了 加 强 算法 对 
围 环 境 的 感知 ， 用 障碍 物 信息 进一步 引导 搜索 ， 本 文 提出 
APFG-RRT 将 人 工 势 场 融入 到 RRT 中 ， 根 据 目 标点 和 周 
弱 障 碍 物 信 息 构建 人 工 势 场 ,引导 RRT 进行 搜索 , 使 扩展 随 
机 树 能 够 更 有 效 地 避 开 障碍 物 ， 从 而 快速 搜索 到 到 达 目 标的 
安全 路 径 。 
在 RRT 中 ,每 次 节点 生长 的 增 量 仅 由 随机 采样 点 x 相 
| www 的 方向 和 步 长 6 决定 , 而 在 APFG-RRT 中 , 生长 的 
量 还 由 机 器 人 在 we 处 受到 的 引力 和 斥 力 决定 。APFG-RRT 
新 节点 的 生成 公式 如 下 : 


还 型 证 工 


> 
Pon 


Krand — Nneares or 


0 7 
其 中 ， Fw 为 机 器 人 在 wis 处 受到 的 引力 和 斥 力 的 合力 ，& 
为 随机 分 量 因 子 ，5 为 势 场 分 量 因 子 。 需 要 注意 的 是 ， 为 了 
保持 算法 的 概率 完备 性 ， 使 随机 树 能 够 到 达 构 型 空间 中 的 任 
意 无 碰撞 位 姿 ，% 和 2 需要 满足 gg<e 。 

与 大 多 数 RRT 等 步 长 的 扩展 方式 不 同 , APFG-RRT 由 随 
机 分 量 和 势 场 分 量 共同 决定 生长 的 距离 ， 当 节点 离 障 碍 物 较 
近 时 ， 势 场 分 量 的 方向 主要 由 斥 力 决定 ， 使 得 向 障碍 物 区 域 
的 生长 步 长 较 小 ， 有 利于 随机 树 通过 狭 窗 区 域 ， 而 当 节 点 远 
离 障碍 物 时 ， 势 场 分 量 的 方向 主要 由 引力 决定 ， 使 得 向 目标 
区 域 的 生长 步 长 较 大 ， 有 利于 快速 接近 目标 。 

APFG-RRT 的 引力 函数 和 斥 力 函 数 分 别 为 


New 已 


d (Xpoa» Memesr) (8) 


202009.00104v1 


chinaXiv 


录用 定稿 
Fy dd , 
和 dral< di 
Po =11+ esl am| (9) 
0 dral> di 
其 中 ,dm = Ow ，Ouw 是 障碍 物 上 离 wwww 最 近 的 点 。 


到 和 F% 分 别 为 引力 常数 和 斥 力 常数 ，k 是 形状 系数 ，di 为 
斥 力 势 场 的 作用 半径 。 
人 工 势 场 法 中 引力 大 小 之 所 以 设置 为 式 (2) 是 为 了 让 机 
器 人 在 远 距 离 时 快速 靠近 目标 ， 而 在 接近 目标 时 防止 机 器 人 
过 冲 。 而 RRT 一 次 规划 出 完整 路 径 , 在 到 达 目 标 附近 时 直接 
将 节点 与 目标 相连 即 可 得 到 路 径 ， 不 会 发 生 过 冲 ， 所 以 
APFG-RRT 中 将 引力 的 大 小 设置 为 常数 F 。 

APFG-RRT 中 斥 力 函 数 为 式 (9), 当 wr 与 障碍 物 的 最 小 
距离 接近 0 时 ， 斥 力 大 小 接近 Fi% ， 当 ww 与 障碍 物 的 最 小 
距离 接近 dw /2 时 ， 斥 力 大 小 接近 Fw 12 ; 当 %wwor 与 障碍 物 的 
最 小 距离 接近 di 时, 斥 力 大 小 接近 0。 相 比 于 式 (4), 式 (9) 中 
参数 的 意义 更 加 直观 ， 斥 力 的 分 布 更 加 合理 。 经 过 测试 ， 斥 
力 函 数 设 为 式 (9) 算 法 的 性 能 更 好 。 

假如 将 Goal-bias RRT 和 APFG-RRT 中 选取 目标 点 作为 
采样 点 的 概率 都 设置 为 100%， 则 Goal-bias RRT 和 APFG- 
RRT 在 简单 场景 中 的 表现 如 图 1 所 示 。Goal-bias RRT 直接 向 
目标 生长 ， 很 容易 在 障碍 物 表 面 陷 入 局 部 极 小 值 ， 需 要 依靠 
概率 为 1-P 的 随机 采样 跳出 局 部 极 小 值 。 而 融合 了 人 工 势 场 


的 APFG-RRT 由 于 加 入 了 斥 力 机 制 , 拥有 了 一 定 的 绕 开 障碍 
物 的 能 力 。 
〇 目标 点 
全 节点 
口 障碍 物 
和 
O 


(a) Goal-Bias RRT 
图 1 Goal-bias RRT 和 APFG-RRT 路 径 规划 示意 图 
Fig. 1 


(b) APFG RRT 


Schematic diagrams of path planning with 
Goal-bias RRT and APFG-RRT 
3.2 自 适应 概率 采样 

Goal-bias RRT 概率 选取 目标 点 作为 采样 点 的 策略 不 仅 
可 以 引导 随机 树 向 目标 生长 ， 还 具有 优先 生长 一 条 分 支 的 特 
性 ， 这 有 利于 快速 规划 出 一 条 路 径 。 但 是 ，Goal-bias RRT 选 


司徒 华 杰 ， 等 : 动态 环境 下 基于 人 工 势 场 引 导 的 RRT 路 径 规划 算法 


ChinaXiv 合 作 期 刊 


第 38 卷 第 3 期 


立 随机 树 的 根 节 点 ， 将 选择 目标 点 作为 采样 点 的 概率 P 设 为 
Rs ， 当 随机 数 小 于 P 时 ,选取 目标 点 作为 采样 点 xwr ， 而 当 
随机 数 大 于 PP 时， 在 机 器 人 构 型 空间 中 随机 采样 。 然 后 选取 
随机 树 上 离 ww 最近 的 节点 xiww 作为 待 生 长 节点 ， 计 算出 障 
碍 物 上 离 we 最 近 的 点 Oi ， 根 据 式 (7) 计 算出 ww ， 判 断 
xu 是 否 与 障碍 物 碰 撞 ， 若 碰撞 且 采 样 点 为 wm ， 则 认为 随机 
树 陷入 局 部 极 小 值 ， 若 无 碰撞 则 将 %e 作为 新 节点 添加 到 随 
机 树 中 , 并 且 如 果 采 样 点 是 xwm, 则 认为 随机 树 没 有 陷入 局 部 
极 小 值 或 者 已 经 跳出 局 部 极 小 值 , 将 P 重新 设 为 PB 。 接着 若 
随机 树 处 于 陷入 局 部 极 小 值 状态 , 则 根据 式 (10) 更 新 P, 不断 
重复 上 述 步骤 ， 直 到 随机 树 生 长 到 目标 点 邻 域 或 者 迭代 次 数 
超过 设 定 的 最 大 值 。 静 态 环境 下 APFG-RRT 的 伪 代 码 如 下 
算法 3 静态 环境 下 的 APFG-RRT 算法 


1:T < InitializeTree(x,,) 


Wh 


2: P=P,,,local __minima = False 
3: for i=0 to K: 

4 if random(0,1)<P: 
5 Nand = Xgoal 

6 else: 

7% Xand = Sample() 
8 Nearest <— NearestNode(T, x,aa ) 

9 Ovarest *— NearestObstacle(Obstacle, Xvaresr» do ) 
106: Xnew €— Steer(Xnoaresr» Xrand» Xgoal» Onearest ) 


11: if CollisionFree(x,,,): 


12: T.add_node(x,,,,) 

13: T.add_edge(Xwaen, Knew ) 
14: if dio, Xooa) <E: 

15: break 

16: if Xan == Xooa : 

7 P= Fa 

18: local _minima = False 
19: else: 

26 : if Xa == Xooa : 

21: n=0 

22: local _minima = True 
23: if local_minima : 

24: P < UpdateProbability(n) 
25: n=n+l 


3.4 动态 环境 下 的 APFG-RRT 算法 

动态 环境 下 路 径 规划 的 难点 在 于 障 人 得 物 可 能 会 运动 到 之 
前 规划 好 的 路 径 上 ， 需 要 算法 在 极 短 时 间 内 重新 规划 出 一 条 
无 碰撞 路 径 ， 因 而 对 算法 的 实时 性 要 求 很 高 。 男 一 方面 ， 考 
虑 到 需要 留 给 机 器 人 一 定 的 避 障 时 间 和 空间 ， 规 划 的 路 径 还 


取 目 标点 作为 采样 点 的 概率 P 是 固定 的 ， 当 随机 树 陷入 局 部 
极 小 值 时 ， 选 取 目 标点 作为 采样 点 的 扩展 一 般 是 无 效 的 ， 这 
会 浪费 大 量 的 计算 资源 。 为 此 ， 本 文 提出 了 一 种 概率 自 适 应 
的 采样 方法 。 当 选取 目标 点 作为 采样 点 的 扩展 无 效 时 ， 可 认 
为 随机 树 陷 入 局 部 极 小 值 ， 将 P 设置 为 0， 使 算法 专注 于 通 
过 随机 生长 跳出 局 部 极 小 值 ， 而 随 着 随机 生长 的 次 数 增加 ， 
随机 树 跳 出 局 部 极 小 值 的 可 能 性 越 来 越 大 ， 所 以 相应 地 P 也 
随 着 随机 生长 次 数 的 增加 而 增加 。 随 机 树 陷 入 局 部 极 小 值 时 
P 的 值 由 下 面 公 式 获得 : 

已 =P (ew ) (10) 


其 中 ， Pw 为 随机 树 未 陷入 局 部 极 小 值 时 选取 目标 点 作为 采 
样 点 的 概率 ，a 是 形状 系数 ，x 是 设 定 的 最 大 尝试 次 数 ， 当 


随机 生长 次 数 接近 w 时 ，P 逐渐 恢复 成 Po 。 
3.3 静态 环境 下 的 APFG-RRT 算法 
APFG-RRT 的 具体 流程 为 : 首先 在 机 器 人 的 初始 位 置 建 


应 远离 障碍 物 。 传 统 的 膨胀 障碍 物 的 方法 虽然 能 使 规划 路 径 
与 障碍 物 保持 一 定 的 安全 距离 ， 但 是 对 于 不 同 场景 需要 调整 
影 胀 参数 ， 比 如 在 狭窄 的 环境 需要 选择 较 小 的 膨胀 程度 ， 否 
则 将 会 使 随机 树 的 扩展 更 加 艰难 。 而 APFG-RRT 由 于 加 入 了 
势 场 对 随机 树 生 长 的 引导 机 制 ， 县 有 更 快 的 收敛 速度 ， 且 生 
成 的 路 径 能 够 相对 远离 障碍 物 , 在 动态 环境 下 有 较 好 的 表现 。 
为 了 进一步 提高 算法 在 动态 环境 下 的 实时 性 ， 本 文 参 考 
RRT*FND P20 中 的 方法 , 使 用 了 局 部 重新 规划 的 策略 一 一 只 对 
在 当前 位 置 一 定 范围 内 未 执行 的 “危险 ”路 径 进行 重新 规划 ， 
从 而 减少 计算 量 。 有 具体 为 先 搜 索 出 在 当前 位 置 半 径 为 范围 
内 的 将 要 执行 的 连续 路 径 ps ， 将 离 障碍 物 最 小 距离 在 必 以 
内 的 节点 认为 是 “危险 ”节点 ， 用 APFG-RRT 对 pis 上 包含 
所 有 “和 危险 ”节点 的 最 短路 径 段 进行 重新 规划 和 连接 。 算 法 
的 伪 代 码 如 算法 4 所 示 。 
算法 4 动态 环境 下 的 APFG-RRT 算法 


录用 定稿 司徒 华 杰 ， 等 : 


1: Obstacle <«— UpdateObstacles() 
2: Pp APFG-RRT(X;, Xe, Obstacle) 


3 

4: While Xeon FXooar : 

5 Pi <— SelectNearPath(p) 

6 Xar» Xend <— DangerousPath(p,a, ) 

7: Pes <— DeletePath(p, Xs , Xona ) 

8 Punansed 人 APFG-RRT(xs, Xoa, Obstacle) 
9 P ¢- AddPath(p,,,,, ponansed ) 


16 : Obstacle *- UpdateObstacles() 
Ts Xent <— GetCurrentState() 


4 ”实验 结果 与 分 析 


为 了 测试 APFG-RRT 的 收敛 速度 , 本 文 对 APFG-RRT 在 
多 种 环境 下 进行 了 仿真 实验 , 并 与 初始 的 RRT 和 Goal-bias RRT 
进行 比较 。 三 种 算法 中 相应 的 参数 保持 相同 。 考 虑 到 RRT 自身 
的 随机 性 ， 在 对 应 一 般 环 境 和 狭窄 环境 的 地 图 上 重复 进行 100 
次 由 起 点 到 目标 点 的 路 径 规划 ， 取 平均 值 作为 测试 结果 。 
图 2 和 3 分别 展示 了 初始 RRT、Goal-bias RRT 和 APFG- 
RRT 在 一 般 场景 和 狭窄 场景 中 的 表现 , 表 1 和 2 则 列 出 了 相 
应 的 实验 数据 。 从 表 中 可 以 看 出 ，Goal-bias RRT 和 APFG- 
RRT 的 表现 明显 优 于 初始 的 RRT， 而 APFG-RRT 在 各 项 指 
标 上 均 优 于 Goal-bias RRT,. 在 地 图 1( 一 般 场景 ) 中 APFG-RRT 
路 径 规划 的 平均 时 间 为 Goal-bias RRT 的 49.3%, 平均 节点 数 
为 Goal-bias RRT 的 41.5%; 在 地 图 2( 狭 窗 场 景 ) 中 APFG-RRT 
路 径 规划 的 平均 时 间 为 Goal-bias RRT 的 47.2%, 平均 节点 数 
为 Goal-bias RRT 的 42.9%。 这 是 因为 人 工 势 场 的 引入 虽然 增 
加 了 算法 每 次 迭代 的 计算 量 , 但 使 APFG-RRT 拥有 绕 开 障 碍 
物 的 能 力 ， 减 少 了 与 障碍 物 发 生 碰撞 的 无 效 节点 数 ， 而 自 适 
应 概率 采样 策略 使 因 陷入 局 部 极 小 值 而 重复 计算 的 无 效 节 点 
数 进一步 减少 。 从 表 中 数据 可 计算 出 Goal-bias RRT 在 地 图 
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表 1 三 种 算法 在 一 般 场景 中 的 实验 结果 


Experimental results of the three algorithms in general scene 


Tab. 1 


算法 RRT Goal-bias RRT APFG-RRT 
平均 规划 时 间 /s 33.81 0.414 0.204 
平均 迭代 次 数 37281 7617 1172 
26895 2428 1008 


平均 节点 数 


(a) Initial RRT 


(b) Goal-bias RRT 
图 3 三 种 算法 在 地 图 2( 狭 窗 环 境 ) 中 的 表现 


Fig.3 了 Performance ofthe three algorithms in 


(c) APFG-RRT 


Map 2(narrow environment) 
表 2 三 种 算法 在 狭窄 环境 中 的 实验 结果 


Tab.2 Experimental results of the three algorithms in 


narrow environment 


算法 RRT Goal-bias RRT APFG-RRT 
平均 规划 时 间 /s 39.869 0.303 0.143 
平均 迭代 次 数 54274 7284 754 
平均 节点 数 27337 1729 742 
图 4 展示 了 APFG-RRT 在 动态 场景 中 使 用 局 部 重新 规划 


策略 后 的 表现 。 图 中 灰色 的 是 运动 障碍 物 ， 它 们 在 固定 
线 上 往复 运动 ， 箭 头 为 它们 在 所 在 帧 中 的 运动 方向 ， 黄 色 粗 
实 线 为 机 器 人 已 经 执行 的 路 径 ， 红 色 点 线 为 上 一 帧 规划 路 径 
中 被 修正 的 部 分 ， 蓝 色 细 实 线 为 机 器 人 在 所 在 帧 中 重新 规划 
后 的 路 径 。 从 图 4 可 以 看 出 ; 加 入 势 场 引导 生长 的 机 制 后 
APFG-RRT 规划 的 路 径 相 对 远离 障碍 物 ， 留 给 了 机 器 人 一 定 
的 避 障 空间 。 值 得 注意 的 是 ， 局 部 重新 规划 能 够 在 一 定 程度 


1、2 中 的 平均 无 效 节点 数 分 别 为 5189 和 5555， 而 APFG- 
RRT 在 地 图 1、2 中 的 平均 无 效 节点 数 仅 为 164 和 12， 这 大 
大 减少 了 算法 无 效 迭 代 所 浪费 的 时 间 。 另 一 方面 ， 随 机 树 上 


上 优化 原来 的 路 径 ， 比 如 在 图 4 的 第 3 张 图 中 重新 规划 的 路 
径 比 原来 的 路 径 更 短 、 更 平滑 ， 并 且 使 原来 由 于 陷入 局 部 极 
小 值 而 靠近 障碍 物 的 路 径 远离 障碍 物 ， 这 是 因为 势 场 的 特性 


Ei 


已 有 的 节点 数 越 多 ， 算 法 每 次 迭代 时 搜索 随机 树 上 离 采 样 点 
最 近 的 节点 消耗 的 时 间 越 多 。Goal-bias RRT 按 概率 1-P 随机 
采样 ， 而 APFG-RRT 在 概率 1-P 时 由 势 场 分 量 和 随机 分 量 共 
同 决定 生长 方向 ， 这 使 得 APFG-RRT 的 扩展 更 有 目的 性 ， 所 
以 APFG-RRT 搜索 到 路 径 时 随机 树 上 的 节点 数 更 少 , 这 减少 
了 平均 每 次 迭代 消耗 的 时 间 。 对 于 狭窄 场景 ，Goal-bias RRT 
因 偏 向 目标 生长 的 特性 而 容易 陷入 贴近 障碍 物 表 面 、 取 目标 
点 为 采样 点 的 扩展 失效 而 需要 依赖 概率 为 1-P 的 随机 采样 逃 
风 的 循环 中 ， 而 在 贴近 狭窄 通道 表面 时 随机 树 的 生长 方向 受 
到 限制 , 支持 有 效 生 长 的 采样 空间 较 小 ; 而 APFG-RRT 因 狭 
窄 通道 中间 的 势能 最 小 ， 容 易 使 随机 树 沿 着 通道 中 间 生 长 ， 

使 得 随机 树 的 生长 方向 受到 的 限制 较 小 ， 有 效 生 长 的 概率 更 
高 ， 分 量 朝 向 通道 表面 时 ， 势 场 分 量 远离 
障 答 物 ， 使 得 生长 步 长 减 小 ， 有 利于 进一步 提高 有 效 生长 的 


Bey 


人 


(a)Initial RRT 
图 2 三 种 算法 在 地 图 1( 一 般 场景 ) 


Fig.2 Performance of the three algorithms in Map 1(general scene) 


(b) Goal-bias RRT (c) APFG-RRT 


中 的 表现 


使 得 APFG- 了 下 gp 单 环 境 中 的 规划 具有 良好 的 表现 。 
国 Lp 网 
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图 4 APFG-RRT 在 动态 环境 中 的 表现 
Fig.4 Performance of APFG-RRT in dynamic environment 
在 实验 中 ，Goal-bias RRT 在 动态 场景 中 的 路 径 规划 采用 
对 每 帧 所 有 路 径 都 进行 重新 规划 的 策略 。 两 种 算法 在 图 4 场 
景 中 规划 100 次 ， 并 取 平 均值 的 规划 结果 如 表 3 所 示 。 
表 3 Goal-bias RRT 和 APFG-RRT 在 动态 环境 中 的 实验 结果 
Tab. 3 ” Experimental results of Goal-bias RRT and APFG-RRT in 


dynamic environment 


算法 Goal-bias RRT APFG-RRT 


阶 
六 


重新 规划 的 平均 时 间 /s 0.421 0.011 
规划 成 功率 0 100% 


司徒 华 杰 ， 等 : 


于 Goal-bias RRT 具有 偏向 目标 点 生长 的 特性 ， 其 搜 
索 到 的 路 径 容 易 在 局 部 极 小 值 区 域 贴 合 障碍 物 ， 可 能 导致 机 
器 人 与 障碍 物 发 生 碰撞 。 如 图 5 所 示 ，Goal-bias RRT 在 当前 
帧 中 重新 规划 的 路 径 虽 然 是 无 碰撞 的 ， 但 是 贴 合 着 障碍 物 ， 
且 在 障碍 物 运 动 方向 上 ， 在 下 一 帧 时 刻 障 碍 物 就 会 与 机 器 人 
发 生 碰撞 ， 这 就 是 导致 Goal-bias RRT 在 实验 的 动态 场景 
规划 成 功率 为 0 的 原因 。 而 对 于 其 他 大 多 数 的 RRT 算法 , 由 
于 没有 远离 障碍 物 的 机 制 ， 规 划 的 路 径 也 有 可 能 会 贴 合 着 障 
碍 物 ， 比 如 图 2(a) 中 初始 RRT 规划 的 路 径 。 

于 Goal-bias RRT 规划 成 功率 为 0， 


NE 
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和 


表 3 中 Goal-bias 


RRT 每 次 重新 规划 的 平均 时 间 是 统计 其 中 成 功 重 新 规划 的 
部 分 得 到 。 从 实验 结果 可 以 看 出 ， 采 用 局 部 重新 规划 策略 的 
APFG-RRT 在 测试 的 动态 场景 中 规划 成 功率 为 100%， 且 每 
次 重新 规划 的 平均 时 间 远 少 于 Goal-bias RRT。 这 是 因为 相 比 
于 在 每 一 帧 时 刻 都 对 全 进行 规 世 局 部 重 
新 规划 使 得 算法 在 每 一 帧 时 刻 只 需 重 于 段 路 径 ， 
当 将 要 执行 的 路 径 安全 时 甚至 不 需要 重新 进行 规划 ， 这 大 大 
减少 了 计算 量 ，T 盾 规 划 策 略 而 额外 增加 的 计算 
量 相 比 之 下 很 小 。 


图 5 Goal-bias RRT 应 用 于 动态 环境 时 容易 在 
局 部 极 小 值 区 域 与 障碍 物 碰撞 
Fig.5 When Goal-bias RRT is applied to dynamic environment, it is 


easy to collide with obstacles in the local minimum area 
5 ”结束 语 


本 文 针对 现 有 的 大 多 数 RRT 动态 路 径 规划 算法 不 能 使 
规划 的 路 径 与 障碍 物 保持 安全 距离 的 问题 ， 提 出 了 一 种 用 人 
工 势 场 引 导 RRT 生长 的 启发 式 算法 , 增强 了 算法 对 周围 环境 
的 感知 能 力 ， 并 结合 自 适应 概率 采样 和 局 部 重新 规划 策略 ， 
使 RRT 的 搜索 效率 进一步 提高 。 仿 真 结果 表明 ， 本 文 提出 的 
改进 RRT 算法 在 静态 和 动态 场景 下 都 有 较为 优越 的 性 能 。 
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